

#include <cegfx/ceeffect.h>
#include <cegfx/cemacros.h>
#include <cegfx/ceskeleton.h>


ceBone::ceBone (const char* name)
	: length (0)
	, name (name)
	, id (0)
{
	IQF_CONSTRUCT;
	SetLength (10);
	SetRotation (D3DXVECTOR3(0, 0, 1), 0);
}


IQF_IMPLEMENTATION_BEGIN(ceBone)
IQF_IMPLEMENTATION_END()

void ceBone::SetId (unsigned id)
{
	this->id = id;
}

unsigned ceBone::GetId () const
{
	return id;
}

const char* ceBone::GetName () const
{
	return name.c_str();
}

void ceBone::SetMatrix (D3DXMATRIX* matrix)
{
	this->matrix = matrix;
}


void ceBone::SetLength (float length)
{
	this->length = length;
}

float ceBone::GetLength () const
{
	return this->length;
}

void ceBone::SetRotation (const D3DXVECTOR3& axis, float angle)
{
	D3DXQuaternionRotationAxis (&quaternion, &axis, angle);
}

const D3DXQUATERNION& ceBone::GetQuaternion () const
{
	return quaternion;
}


void ceBone::AddBone (ceBone* bone)
{
	if (!bone)
	{
		return;
	}

	bone->AddRef ();
	children.push_back (bone);

	bone->parent = this;
}

void ceBone::RemoveBone (const ceBone* bone)
{
	FOR_VECTOR(unsigned, i, j, children)
	{
		ceBone* child = children[i];
		if (child == bone)
		{
			if (child->parent == this)
			{
				child->parent = 0;
			}
			children.erase (children.begin () + i);
			child->Release();
			return;
		}
	}
}

void ceBone::Update (const D3DXMATRIX& base)
{
	// calculate the matrix of the bone
	// this is the matrix describing the bottom of the bone
	D3DXMatrixIdentity(matrix);
	D3DXMatrixRotationQuaternion(matrix, &quaternion);
	// XXX: this might be other way round
	D3DXMatrixMultiply (matrix, matrix, &base);

	// calculate the head of the bone.
	// that is the point where the child bones are connected
	D3DXMATRIX tmp;
	D3DXMatrixIdentity(&tmp);
	D3DXMatrixTranslation (&tmp, 0, 0, length);
	D3DXMatrixMultiply (&tmp, &tmp, matrix);

	FOR_VECTOR_UIJ(children)
	{
		children[i]->Update (tmp);
	}
}

void ceBone::Debug ()
{
	printf ("Bone [%s]\n", name.c_str());
	printf ("    %.2f  %.2f  %.2f  %.2f\n", matrix->_11, matrix->_12, matrix->_13, matrix->_14);
	printf ("    %.2f  %.2f  %.2f  %.2f\n", matrix->_21, matrix->_22, matrix->_23, matrix->_24);
	printf ("    %.2f  %.2f  %.2f  %.2f\n", matrix->_31, matrix->_32, matrix->_33, matrix->_34);
	printf ("    %.2f  %.2f  %.2f  %.2f\n", matrix->_41, matrix->_42, matrix->_43, matrix->_44);
	printf ("\n");

	FOR_VECTOR_UIJ(children)
	{
		children[i]->Debug ();
	}
}

ceSkeleton::ceSkeleton ()
	: matrices (0)
{
	IQF_CONSTRUCT;
}

IQF_IMPLEMENTATION_BEGIN(ceSkeleton)
IQF_IMPLEMENTATION_INTERFACE(iGeometryAnimator)
IQF_IMPLEMENTATION_END()


ceBone* ceSkeleton::CreateBone (const char* name, float length, bool rootBone)
{
	ceBone* bone = new ceBone (name);
	bone->SetLength (length);

	bone->AddRef ();
	bones.push_back (bone);


	if (rootBone)
	{
		rootBones.push_back (bone);
	}

	return bone;
}


ceBone* ceSkeleton::CreateBone (const char* name, float length, ceBone* parent)
{
	ceBone* bone = CreateBone (name, length, false);

	if (parent)
	{
		parent->AddBone (bone);
	}

	return bone;
}

ceBone* ceSkeleton::GetBone (unsigned idx) 
{
	if (idx >= bones.size ())
	{
		return 0;
	}

	return bones[idx];
}

const ceBone* ceSkeleton::GetBone (unsigned idx) const 
{
	if (idx >= bones.size ())
	{
		return 0;
	}

	return bones[idx];
}

ceBone* ceSkeleton::GetBone (const char* name)
{
	FOR_VECTOR_UIJ(bones)
	{
		if (strcmp (bones[i]->GetName (), name) == 0)
		{
			return bones[i];
		}
	}
	return 0;
}

const ceBone* ceSkeleton::GetBone (const char* name) const 
{
	FOR_VECTOR_UIJ(bones)
	{
		if (strcmp (bones[i]->GetName (), name) == 0)
		{
			return bones[i];
		}
	}
	return 0;
}

void ceSkeleton::Build ()
{
	if (matrices)
	{
		delete [] matrices;
	}

	matrices = new D3DXMATRIX[bones.size ()];

	// TODO: sort the bones

	FOR_VECTOR_UIJ(bones)
	{
		ceBone* bone = bones[i];
		bone->SetId (i);
		bone->SetMatrix (&matrices[i]);
	}

}

void ceSkeleton::Update ()
{
	D3DXMATRIX mat;
	D3DXMatrixIdentity (&mat);
	Update (mat);
}

void ceSkeleton::Update (const D3DXMATRIX& base)
{
	FOR_VECTOR_UIJ(rootBones)
	{
		ceBone* bone = rootBones[i];
		bone->Update (base);
	}
}

void ceSkeleton::Debug ()
{
	printf ("Skeleton\n");
	FOR_VECTOR_UIJ(rootBones)
	{
		rootBones[i]->Debug ();
	}
}


unsigned ceSkeleton::GetNumberOfBones () const
{
	return bones.size ();
}

const D3DXMATRIX* ceSkeleton::GetBoneMatrices () const
{
	return matrices;
}


void ceSkeleton::UpdateGeometry (iGeometry* geometry)
{
}

void ceSkeleton::UpdateEffectStates (ceEffect* effect)
{
	if (effect)
	{
		D3DXHANDLE handle = effect->GetHandleBySemantic ("BONEMATRICES");
		if (handle)
		{
			/*
			printf ("Setup bone matrices\n");
			printf ("  %.2f %.2f %.2f %.2f\n", matrices[0]._11, matrices[0]._12, matrices[0]._13, matrices[0]._14);
			printf ("  %.2f %.2f %.2f %.2f\n", matrices[0]._21, matrices[0]._22, matrices[0]._23, matrices[0]._24);
			printf ("  %.2f %.2f %.2f %.2f\n", matrices[0]._31, matrices[0]._32, matrices[0]._33, matrices[0]._34);
			printf ("  %.2f %.2f %.2f %.2f\n", matrices[0]._41, matrices[0]._42, matrices[0]._43, matrices[0]._44);
			printf ("\n\n");
			*/
			effect->SetMatrices (handle, bones.size(), matrices);
		}
	}
}
